#include "vision.h"

int Vision::cursorx, Vision::cursory;
bool Vision::clicked;
void On_Mouse(int event, int x, int y, int flags, void* userdata)
{
    using namespace cv;
	int b, g, r;
	switch (event) {
	case EVENT_MOUSEMOVE:
        Vision::cursorx = x;
        Vision::cursory = y;
        break;
	case EVENT_LBUTTONUP:
        Vision::clicked = true;
		break;
	default:break;
	}
}

zhnmat::Mat cvMat2zhnMat(const cv::Mat& m)
{
    zhnmat::Mat ans(m.rows, m.cols);
    for (int i=0; i<m.rows; i++)
            for (int j=0; j<m.cols; j++)
                ans.set(i, j, m.at<double>(i, j));
    return ans;
}

Vision::Vision()
{
    ImgShow = false;
    detector = cv::xfeatures2d::SURF::create();
    detector->setHessianThreshold(400);
    cameraMatrix1 = (cv::Mat_<double>(3, 3)<<416.3231506347656, 0, 329.717362414036, 0, 417.3470764160156, 244.3128579594722, 0, 0, 1);
    cameraMatrix2 = (cv::Mat_<double>(3, 3)<<412.943888642234, 0, 337.0526611825449, 0, 414.4333009782604, 237.5232967036032, 0, 0, 1);
    distCoeffs1 = (cv::Mat_<double>(1, 5)<<-0.02610082408985964, 0.0245343887967647, -0.0009149796963914672, -0.0002431880798190779, 0);
    distCoeffs2 = (cv::Mat_<double>(1, 5)<<-0.02341011172972657, 0.02244382615595914, 0.0006435773627936445, -0.002869524842257356, 0);
    R1 = (cv::Mat_<double>(3, 3)<<0.9999422515884582, 0.003478968695372097, -0.01016810036443274,
        -0.003496876809996775, 0.9999923652029409, -0.001743957684125892,
        0.01016195555886085, 0.00177941356770594, 0.9999467827572499);
    R2 = (cv::Mat_<double>(3, 3)<<0.9999966595490133, 0.002415975400461994, 0.0009186695157186947,
        -0.002417590082908963, 0.9999955277593123, 0.001760602046341046,
        -0.0009144118359735443, -0.001762817131446878, 0.9999980281614336);
    P1 = (cv::Mat_<double>(3, 3)<<404.9269208768735, 0, 336.058952331543, 0, 404.9269208768735, 240.7664337158203, 0, 0, 1);
    P2 = (cv::Mat_<double>(3, 3)<<404.9269208768735, 0, 333.7297821044922, 0, 404.9269208768735, 240.7664337158203, 0, 0, 1);
    WR = zhnmat::Mat(3, 3, std::vector<double>{0.1529212370240064, 0.08021120948107502, -0.9907702579006981, 0.9923117712779479, 0.05160393021086301, 0.1519641595785162, 0.05811102004141788, -1.000670734600931, -0.06737141109162714});
    WT = zhnmat::Mat(std::vector<double>{0.1896823391318321, 2.742277580639347, 2.383115116227418})*100;
    TtimesF = 48722.26225851812;
    zhnP1 = cvMat2zhnMat(P1);
    clicked = false;
    initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, CV_16SC2, mapl1, mapl2);
    initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize, CV_16SC2, mapr1, mapr2);
}

void Vision::Add_ImageFromFile(const std::string imagePath)
{
    cv::Mat box = cv::imread(imagePath);
    objCorners.clear();
    detector->detectAndCompute(box, cv::Mat(), objKeypoints, objDescriptors);
    objCorners.push_back(cv::Point2f(0, 0));
    objCorners.push_back(cv::Point2f(box.cols, 0));
    objCorners.push_back(cv::Point2f(box.cols, box.rows));
    objCorners.push_back(cv::Point2f(0, box.rows));
}

int Vision::ImgShow_Init()
{
    cap = cv::VideoCapture(0);
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    imgSaveCnt = 1;
    if (!cap.isOpened()) {
        cv::destroyAllWindows();
        return 1;
    };
    return 0;
}

int Vision::ImgShow_Update(bool calibra)
{
    char ch, s[12];
    cv::Mat frame;
    if (!cap.read(frame)) {
        cv::destroyAllWindows();
        return 2;
    }
    if (calibra){
        cv::Mat imgL, imgR;
        Camera_Calibrate(frame, imgL, imgR);
        hconcat(imgL, imgR, frame);
    }
    cv::imshow("CameraTest", frame);
    cv::setMouseCallback("CameraTest", On_Mouse);
    ch = (char)cv::waitKey(10);
    if (ch == 's') {
        sprintf(s, "test%d.png", imgSaveCnt);
        cv::imwrite(s, frame);
        imgSaveCnt++;
    }
    return 0;
}

void Vision::ImgShow_Exit()
{
    cv::destroyAllWindows();
    cap.release();
}

void Vision::Camera_Calibrate(const cv::Mat& src, cv::Mat& imgL, cv::Mat& imgR)
{
    remap(src(cv::Rect(0, 0, IMAGE_WIDTH, IMAGE_HIGHT)), imgL, mapl1, mapl2, cv::INTER_LINEAR);
    remap(src(cv::Rect(IMAGE_WIDTH, 0, IMAGE_WIDTH, IMAGE_HIGHT)), imgR, mapr1, mapr2, cv::INTER_LINEAR);
}

int Vision::Detect_Once(cv::Mat& src)
{
    using namespace cv;
    using namespace cv::xfeatures2d;
    using namespace std;
    std::vector<KeyPoint> keypoints;
    Mat descriptors;
    FlannBasedMatcher matcher;
    vector<DMatch> matches, goodMatches;
    detector->detectAndCompute(src, Mat(), keypoints, descriptors);
    try { matcher.match(objDescriptors, descriptors, matches); }
    catch (const exception &) { return 1; }
    double dist,  mindist = 100;
    for (int i = 0; i < objDescriptors.rows; i++) {
        dist = matches[i].distance;
        if (dist < mindist)
            mindist = dist;
    }
    for (int i = 0; i < objDescriptors.rows; i++)
        if (matches[i].distance <= max(3 * mindist, 0.08))
            goodMatches.push_back(matches[i]);
    vector<Point2f>vecobj, vecsce;
    for (int i = 0; i < goodMatches.size(); i++) {
        vecobj.push_back(objKeypoints[goodMatches[i].queryIdx].pt);
        vecsce.push_back(keypoints[goodMatches[i].trainIdx].pt);
    }
    try { H = findHomography(vecobj, vecsce, RHO); }
    catch (const exception &) { return 2; }
    vector<Point2f>sceneCorners(4);
    try { perspectiveTransform(objCorners, sceneCorners, H); }
    catch (const exception &) { return 3; }
    cv::line(src, sceneCorners[0], sceneCorners[1], Scalar(0, 255, 255), 2);
    cv::line(src, sceneCorners[1], sceneCorners[2], Scalar(0, 255, 255), 2);
    cv::line(src, sceneCorners[2], sceneCorners[3], Scalar(0, 255, 255), 2);
    cv::line(src, sceneCorners[3], sceneCorners[0], Scalar(0, 255, 255), 2);
    return 0;
}

int Vision::Point_Perspective(const cv::Point2f& boxPoint, cv::Point2f& scenePoint)
{
    using namespace std;
    // using namespace cv;
    std::vector<cv::Point2f> boxes, scenes(1);
    boxes.push_back(boxPoint);
    try { perspectiveTransform(boxes, scenes, H); }
    catch (const exception &) { return 1; }
    // cv::perspectiveTransform(boxes, scenes, H);
    scenePoint = scenes[0];
    return 0;
}

zhnmat::Vector3d Vision::Cal_LocationCamera(double xl, double xr, double yl)
{
    double Z = TtimesF / (xl - xr);
    return zhnP1.inv() * zhnmat::Vector3d(xl, yl, 1) * Z;
}

zhnmat::Vector3d Vision::Cal_LocationWorld(const zhnmat::Vector3d& v)
{
    return WR * v + WT;
}
